#!/usr/bin/env python  
"""
My solution for Assignment 2, Part 2, Task 2 in COMP 4766, Winter 2014.

NOTE: on my system, Stage seems to hang periodically during animation, despite debug output from ROS
indicating that everything is fine. In this particular controller, sometimes as the robot approaches its goal,
it hangs before it reaches the correct pose, giving the appearance that the controller is incorrect. However,
waiting a few seconds, one can see that the robot does resume animation, and rotates to face the correct direction.

Name: Daniel Cook
Email: dac456@mun.ca
Student number: 
"""

from geometry_msgs.msg import Twist
import math
import rospy

class SmoothController2:
    def __init__(self, goalIx, goalIy, goalIa):
        """
        Initialize the class with the position of the goal in the robot ref.
        frame (goalIx, goalIy).
        """
        self.goalIx = goalIx
        self.goalIy = goalIy
        self.goalIa = goalIa;

        # initially, the goal wrt the robot and the goal wrt the world are the same
        self.goalRx = goalIx
        self.goalRy = goalIy
        self.goalRa = goalIa;
  
    """
    Helper method for calculating dot product (for coordinate transformation)
    """
    def dot_product(self, x1, y1, x2, y2):
        return (x1*x2)+(y1*y2)
    
    """
    Dot product in three dimensions
    """
    def dot_product3(self, x1, y1, z1, x2, y2, z2):
        return (x1*x2)+(y1*y2)+(z1*z2)
    
    """
    Helper method to convert cartesian coordinates to polar coordinates
    """
    def cart_to_polar(self, x, y):
        rho = math.sqrt(x**2 + y**2)
        alpha = math.atan(y/x) #unused, see comments in get_twist(...)
        alpha2 = math.atan2(y, x)
        
        return (rho, alpha2)

    def get_twist(self, x, y, theta):
        """
        Given the current pose of the robot (x, y, theta) compute and return
        the appropriate Twist message.
        """
        t = Twist()
        t.linear.x = 0
        t.angular.z = 0
        
        polar = self.cart_to_polar(self.goalRx, self.goalRy)
        
        v = 0.6*polar[0]
        w = 1.6*polar[1] - 0.3*self.goalRa
        
        if(polar[0] != 0):
            t.linear.x = v
        
        """
        logic differs slightly from notes to allow use of atan2, which is the only way I could get this to work
        since atan2 takes the sign of the arguments into account, it is not enough to check alpha > 0, since alpha is allowed to be
        negative. thus, my code checks that alpha is not zero.
        """    
        if(polar[1] != 0):
            t.angular.z = w
            
        gdx = self.goalIx - x
        gdy = self.goalIy - y
        gda = self.goalIa - theta
        
        self.goalRx = self.dot_product3(math.cos(theta), math.sin(theta), 0, gdx, gdy, gda)
        self.goalRy = self.dot_product3(-math.sin(theta), math.cos(theta), 0, gdx, gdy, gda)
        self.goalRa = self.dot_product3(0, 0, 1, gdx, gdy, gda)
        
        rospy.loginfo("goalRa: %f", self.goalRa)
        
        return t
